#include "s_mpu9250.h"


#define S_MPU9250_ADDR						0xD0

#define S_MPU9250_WHOAMI_REG				0x75
#define S_MPU9250_WHOAMI_VALUE				0xBF

#define S_MPU9250_PWR_MGMT_1_REG			0x6B
#define S_MPU9250_PWR_MGMT_1_andValue		0x8F
#define S_MPU9250_PWR_MGMT_1_orValue		0x20
#define S_MPU9250_PWR_MGMT_1_HRESET			0x80

#define S_MPU9250_PWR_MGMT_2_REG			0x6C
#define S_MPU9250_PWR_MGMT_2_andValue		0xC7
#define S_MPU9250_PWR_MGMT_2_orValue		0x07

#define S_MPU9250_ACCEL_CONFIG_2_REG		0x1D
#define S_MPU9250_ACCEL_CONFIG_2_orValue	0b00000101

#define S_MPU9250_INT_PIN_CFG_REG			0x37
#define S_MPU9250_INT_PIN_CFG_Value			0x20

#define S_MPU9250_INT_ENABLE_REG			0x38
#define S_MPU9250_INT_ENABLE_Value			0x40

#define S_MPU9250_INT_STATUS_REG			0x3A
#define S_MPU9250_INT_STATUS_WakeOnMotion	0x40

#define S_MPU9250_MOT_DETECT_CTRL_REG		0x69
#define S_MPU9250_MOT_DETECT_CTRL_orValue	0b11000000

#define S_MPU9250_WOM_THR_REG				0x1F
#define S_MPU9250_WOM_THR_Value				0x04//0xFF

#define S_MPU9250_LP_ACCEL_ODR_REG			0x1E
#define S_MPU9250_LP_ACCEL_ODR_Value		0x07//0b11100000

#define S_MPU9250_ACCEL_OUT_REG				0x3B
#define S_MPU9250_ACCEL_OUT_LEN				6

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

 #include "h_cc2640r2f_iics.h"
 #include "h_cc2640r2f_define.h"

 #include <ti/devices/cc26x0r2/driverlib/ioc.h>

 #define s_mpu9250_dbg					H_CC2640R2F_UART_Debug

 static signed char s_mpu9250_writebuff(unsigned char reg, unsigned char *wdata, unsigned short len)
 {
 	return H_CC2640R2F_IICS1_WriteBuff(S_MPU9250_ADDR, reg, wdata, len);
 }

 static signed char s_mpu9250_readbuff(unsigned char reg, unsigned char *rdata, unsigned short len)
 {
 	return H_CC2640R2F_IICS1_ReadBuff(S_MPU9250_ADDR, reg, rdata, len);
 }

 static void s_mpu9250_delay_us(unsigned int dly)
 {
     unsigned char i = 0;
     for(i = 0U; i < 12U; i ++)
     {
         CPUdelay(dly);
     }

 }

 static void s_mpu9250_delay_ms(unsigned int dly)
 {
     unsigned short i = 0U;
     for(i = 0U; i < 1000; i++)
     {
         s_mpu9250_delay_us(dly);
     }
 }

//#include "h_stm32_hal_iic.h"
//
//#include "h_stm32_hal_sys.h"
//#include "h_stm32_hal_uart.h"
//
//#define s_mpu9250_dbg					H_STM32_HAL_Debug
//
//static signed char s_mpu9250_writebuff(unsigned char reg, unsigned char *wdata, unsigned short len)
//{
//	return H_STM32_HAL_IIC_WriteBuff(S_MPU9250_ADDR, reg, wdata, len);
//}
//
//static signed char s_mpu9250_readbuff(unsigned char reg, unsigned char *rdata, unsigned short len)
//{
//	return H_STM32_HAL_IIC_ReadBuff(S_MPU9250_ADDR, reg, rdata, len);
//}
//
//static void s_mpu9250_delay_ms(unsigned int dly)
//{
//    HAL_Delay(dly);
//}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


signed char S_MPU9250_Check_WhoAmI(void)
{
	unsigned char wai = 0U;
	s_mpu9250_readbuff(S_MPU9250_WHOAMI_REG, &wai, 1);

	s_mpu9250_dbg("The who am i is 0x%02x\r\n", wai);
	return wai;
}

signed char S_MPU9250_H_Reset(void)
{
	//signed char rslt = 0;
	unsigned char data = S_MPU9250_PWR_MGMT_1_HRESET;
	return s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_1_REG, &data, 1);
}

signed char S_MPU9250_Get_Acc_Datas(void)
{
	signed char rslt = 0;

	unsigned char data[S_MPU9250_ACCEL_OUT_LEN];
	signed short datas[3];

	rslt = s_mpu9250_readbuff(S_MPU9250_ACCEL_OUT_REG, data, S_MPU9250_ACCEL_OUT_LEN);

	datas[0] = (signed short)(data[0] << 8) | (data[1]);
	datas[1] = (signed short)(data[2] << 8) | (data[3]);
	datas[2] = (signed short)(data[4] << 8) | (data[5]);

	s_mpu9250_dbg("The datas is %d, %d, %d, rslt is %d\r\n", datas[0], datas[1], datas[2],rslt);
	return rslt;
}

signed char S_MPU9250_wakeOn_Motion_Int(void)
{
	signed char rslt = 0;

	unsigned char data[5];

	//Make sure acc is running
	data[0] = 0x00;
	rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	s_mpu9250_delay_ms(10);

	data[0] = 0x07;
	rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_2_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Set acc LPF setting to 184Hz Bandwidth
	data[0] = 0x05;
	rslt += s_mpu9250_writebuff(S_MPU9250_ACCEL_CONFIG_2_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Enable Motion Interrupt
	data[0] = S_MPU9250_INT_ENABLE_Value;
	rslt += s_mpu9250_writebuff(S_MPU9250_INT_ENABLE_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Enable acc Hardware Intelligence
	data[0] = 0xC0;
	rslt += s_mpu9250_writebuff(S_MPU9250_MOT_DETECT_CTRL_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Set Motion Thershold
	data[0] = S_MPU9250_WOM_THR_Value;
	rslt += s_mpu9250_writebuff(S_MPU9250_WOM_THR_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Set Frequency of Wake-up
	data[0] = S_MPU9250_LP_ACCEL_ODR_Value;
	rslt += s_mpu9250_writebuff(S_MPU9250_LP_ACCEL_ODR_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Set INT Pin
	data[0] = S_MPU9250_INT_PIN_CFG_Value;
	rslt += s_mpu9250_writebuff(S_MPU9250_INT_PIN_CFG_REG, data, 1);
	s_mpu9250_delay_ms(10);

	//Enable Cycle Mode(Accel Low Power Mode)
	data[0] = 0x20;
	rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	s_mpu9250_delay_ms(10);

	// //Make sure acc is running
	// rslt = s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	// data[0] &= S_MPU9250_PWR_MGMT_1_andValue;
	// rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);

	// rslt += s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_2_REG, data, 1);
	// data[0] &= S_MPU9250_PWR_MGMT_2_andValue;
	// data[0] |= S_MPU9250_PWR_MGMT_2_orValue;
	// rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_2_REG, data, 1);

	// //Set acc LPF setting to 184Hz Bandwidth
	// rslt += s_mpu9250_readbuff(S_MPU9250_ACCEL_CONFIG_2_REG, data, 1);
	// data[0] |= S_MPU9250_ACCEL_CONFIG_2_orValue;
	// rslt += s_mpu9250_writebuff(S_MPU9250_ACCEL_CONFIG_2_REG, data, 1);

	// //Enable Motion Interrupt
	// data[0] = S_MPU9250_INT_ENABLE_Value;
	// rslt += s_mpu9250_writebuff(S_MPU9250_INT_ENABLE_REG, data, 1);

	// //Enable acc Hardware Intelligence
	// rslt += s_mpu9250_readbuff(S_MPU9250_MOT_DETECT_CTRL_REG, data, 1);
	// data[0] |= S_MPU9250_MOT_DETECT_CTRL_orValue;
	// rslt += s_mpu9250_writebuff(S_MPU9250_MOT_DETECT_CTRL_REG, data, 1);

	// //Set Motion Thershold
	// data[0] = S_MPU9250_WOM_THR_Value;
	// rslt += s_mpu9250_writebuff(S_MPU9250_WOM_THR_REG, data, 1);

	// //Set Frequency of Wake-up
	// data[0] = S_MPU9250_LP_ACCEL_ODR_Value;
	// rslt += s_mpu9250_writebuff(S_MPU9250_LP_ACCEL_ODR_REG, data, 1);

	// //Set INT Pin
	// // data[0] = S_MPU9250_INT_PIN_CFG_Value;
	// // rslt += s_mpu9250_writebuff(S_MPU9250_INT_PIN_CFG_REG, data, 1);

	// //Enable Cycle Mode(Accel Low Power Mode)
	// rslt += s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	// data[0] |= S_MPU9250_PWR_MGMT_1_orValue;
	// rslt += s_mpu9250_writebuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);


	s_mpu9250_dbg("Wake on rslt is %d\r\n", rslt);

	return rslt;
}

signed char S_MPU9250_wakeOn_Motion_Check(void)
{
	signed char rslt = 0;

	unsigned char data[5];

	//Make sure acc is running
	rslt = s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_PWR_MGMT_1_REG : 0x%02x\r\n", data[0]);


	rslt += s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_2_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_PWR_MGMT_2_REG : 0x%02x\r\n", data[0]);
	
	//Set acc LPF setting to 184Hz Bandwidth
	rslt += s_mpu9250_readbuff(S_MPU9250_ACCEL_CONFIG_2_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_ACCEL_CONFIG_2_REG : 0x%02x\r\n", data[0]);

	//Enable Motion Interrupt
	rslt += s_mpu9250_readbuff(S_MPU9250_INT_ENABLE_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_INT_ENABLE_REG : 0x%02x\r\n", data[0]);

	//Enable acc Hardware Intelligence
	rslt += s_mpu9250_readbuff(S_MPU9250_MOT_DETECT_CTRL_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_MOT_DETECT_CTRL_REG : 0x%02x\r\n", data[0]);

	//Set Motion Thershold
	rslt += s_mpu9250_readbuff(S_MPU9250_WOM_THR_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_WOM_THR_REG : 0x%02x\r\n", data[0]);

	//Set Frequency of Wake-up
	rslt += s_mpu9250_readbuff(S_MPU9250_LP_ACCEL_ODR_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_LP_ACCEL_ODR_REG : 0x%02x\r\n", data[0]);

	//Set INT Pin
	// data[0] = S_MPU9250_INT_PIN_CFG_Value;
	// rslt += s_mpu9250_writebuff(S_MPU9250_INT_PIN_CFG_REG, data, 1);

	//Enable Cycle Mode(Accel Low Power Mode)
	rslt += s_mpu9250_readbuff(S_MPU9250_PWR_MGMT_1_REG, data, 1);
	s_mpu9250_dbg("The S_MPU9250_PWR_MGMT_1_REG : 0x%02x\r\n", data[0]);


	s_mpu9250_dbg("Wake on check rslt is %d\r\n", rslt);

	return rslt;
}

unsigned char S_MPU9250_Read_INT_STATUS(void)
{
	unsigned char status = 0U;
	s_mpu9250_readbuff(S_MPU9250_INT_STATUS_REG, &status, 1);

	return status;

}

signed char S_MPU9250_WakeOn_Mostion_Handle(void)
{
	signed char rslt = 0;

	
	unsigned char int_status = S_MPU9250_Read_INT_STATUS();


	s_mpu9250_dbg("Int status is 0x%02x\r\n", int_status);

	if(S_MPU9250_INT_STATUS_WakeOnMotion & int_status)
	{
		S_MPU9250_Get_Acc_Datas();
	}

	// s_mpu9250_delay_ms(10);

	
	return rslt;
}

void S_MPU9250_Test(void)
{
	S_MPU9250_H_Reset();
	s_mpu9250_delay_ms(1000);

	S_MPU9250_Check_WhoAmI();

	S_MPU9250_wakeOn_Motion_Int();

	S_MPU9250_wakeOn_Motion_Check();

	// while(1)
	// {
	// 	S_MPU9250_Get_Acc_Datas();
	// 	// S_MPU9250_Check_WhoAmI();
	// 	s_mpu9250_delay_ms(10);
	// }
	
}
